/**
 ****************************(C) COPYRIGHT 2019 DJI****************************
* @file       shoot.c/h
* @brief      射击功能.
* @note       
* @history
*  Version    Date            Author          Modification
*  V1.0.0     Dec-26-2018     RM              1. 完成
*
@verbatim
==============================================================================

==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2019 DJI****************************
*/

#include "shoot.h"
#include "main.h"

#include "cmsis_os.h"

#include "bsp_laser.h"
#include "bsp_fric.h"
#include "arm_math.h"
#include "user_lib.h"
#include "referee.h"

#include "CAN_receive.h"
#include "gimbal_behaviour.h"
#include "detect_task.h"
#include "pid.h"

#define shoot_fric1_on(pwm) fric1_on((pwm)) //摩擦轮1pwm宏定义
#define shoot_fric2_on(pwm) fric2_on((pwm)) //摩擦轮2pwm宏定义
#define shoot_fric_off()    fric_off()      //关闭两个摩擦轮


/*射击状态机设置，遥控器上拨一次开启，再上拨关闭，下拨1次发射1颗，一直处在下，则持续发射，用于3min准备时间清理子弹*/
static void shoot_set_mode(void);
/*射击数据更新*/
static void shoot_feedback_update(void);

/*堵转倒转处理*/
static void trigger_motor_turn_back(void);

shoot_control_t shoot_control;          //射击数据
int shoot_time = 0;


/* 发射机构任务*/
void shoot_task(void const *pvParameters)
{
    //射击初始化
    shoot_init();
    while(1)
    {	
        vTaskDelay(2);
        shoot_set_mode();		    //模式设置
        shoot_feedback_update();  //更新
        shoot_control_loop();	    //射击循环
    }

}

/*射击初始化，初始化PID，遥控器指针，电机指针*/
void shoot_init(void)
{
    static const fp32 Trigger_speed_pid[3] = {TRIGGER_ANGLE_PID_KP, TRIGGER_ANGLE_PID_KI, TRIGGER_ANGLE_PID_KD};
    shoot_control.shoot_mode = SHOOT_STOP;
    //遥控器指针
    shoot_control.shoot_rc = get_remote_control_point();
    //电机指针
    shoot_control.shoot_motor_measure = GetDjiMotorMeasurePoint(2, 6);
    //初始化PID
    PID_init(&shoot_control.trigger_motor_pid, PID_POSITION, Trigger_speed_pid, TRIGGER_READY_PID_MAX_OUT, TRIGGER_READY_PID_MAX_IOUT);
    //更新数据
    shoot_feedback_update();
    //斜坡
    ramp_init(&shoot_control.fric1_ramp, SHOOT_CONTROL_TIME * 0.001f, FRIC_UP, FRIC_OFF);
    ramp_init(&shoot_control.fric2_ramp, SHOOT_CONTROL_TIME * 0.001f, FRIC_UP, FRIC_OFF);
    //初始化相关变量
    shoot_control.fric_pwm1 = FRIC_OFF;
    shoot_control.fric_pwm2 = FRIC_OFF;
    shoot_control.ecd_count = 0;
    shoot_control.angle = shoot_control.shoot_motor_measure->ecd * MOTOR_ECD_TO_ANGLE;
    shoot_control.given_current = 0;
    shoot_control.move_flag = 0;
    shoot_control.set_angle = shoot_control.angle;
    shoot_control.speed = 0.0f;
    shoot_control.speed_set = 0.0f;
    shoot_control.key_time = 0;
}

static void shoot_set_mode(void)
{
    if(get_shoot_power() == 0){
        shoot_control.shoot_mode = SHOOT_STOP;
        shoot_control.fric1_ramp.out = 1000;
        shoot_control.fric2_ramp.out = 1000;
        return;
    }
    //上，拨弹，用于调试
    if(switch_is_up(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
    {
        //设置让摩擦轮速度达到一定值之后再进行发弹
        if(shoot_control.fric_pwm1 >= SHOOT_CRITICAL_SPEED &&
            shoot_control.fric_pwm2 >= SHOOT_CRITICAL_SPEED)
        shoot_control.shoot_mode = SHOOT_FIRE;
    }
	//中，开启摩擦轮---上场直接开启摩擦轮，就不关了
	else if(switch_is_mid(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
	{
        //等待2006上电
        if(shoot_time < 20)
        {
            vTaskDelay(2);
            shoot_time++;
        }
        else
        {
            shoot_control.shoot_mode = SHOOT_READY_FRIC;
            shoot_time = 0;
        }
        
        //---如果在中档模式下按下左键，开火
        if (shoot_control.press_l)
        {
            shoot_control.shoot_mode = SHOOT_FIRE;
        }
	} 
    //下停止
	else if(switch_is_down(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
	{
		shoot_control.shoot_mode = SHOOT_STOP;
	}
        
    //为防止程序错误导致不能发弹，所以取消这个超热量限制
    //发弹超热量
    // if(!toe_is_error(REFEREE_TOE) && (shoot_control.heat + SHOOT_HEAT_REMAIN_VALUE > shoot_control.heat_limit))
    // {
    //     if(shoot_control.shoot_mode == SHOOT_FIRE)
    //     {
    //         shoot_control.shoot_mode = SHOOT_READY_FRIC;
    //     }
    // }
}

int16_t shoot_control_loop(void)
{
    //根据模式设置2006拨弹电机的速度
    if (shoot_control.shoot_mode == SHOOT_STOP)
    {
        //设置拨弹轮的速度
        shoot_control.speed_set = 0.0f;
    }
    else if (shoot_control.shoot_mode == SHOOT_READY_FRIC)
    {
        //设置拨弹轮的速度
        shoot_control.speed_set = 0.0f;

    }
    //
    else if (shoot_control.shoot_mode == SHOOT_FIRE)
    {
        //设置拨弹轮的拨动速度,并开启堵转反转处理
        shoot_control.trigger_speed_set = CONTINUE_TRIGGER_SPEED;
        
        trigger_motor_turn_back();
        shoot_control.trigger_motor_pid.max_out = TRIGGER_READY_PID_MAX_OUT;
        shoot_control.trigger_motor_pid.max_iout = TRIGGER_READY_PID_MAX_IOUT;
    }

    //跟据模式设置摩擦轮速度
    if(shoot_control.shoot_mode == SHOOT_STOP)//关闭摩擦轮
    {
        shoot_control.given_current = 0;//拨弹电流值为0
        
        //摩擦轮需要一个个斜波开启，不能同时直接开启，否则可能电机不转
        //关闭摩擦轮
        ramp_calc(&shoot_control.fric1_ramp, -SHOOT_FRIC_PWM_ADD_VALUE);
        ramp_calc(&shoot_control.fric2_ramp, -SHOOT_FRIC_PWM_ADD_VALUE);
    }
    else
    {
        //计算拨弹轮电机PID
        PID_calc(&shoot_control.trigger_motor_pid, shoot_control.speed, shoot_control.speed_set);
        shoot_control.given_current = (int16_t)(shoot_control.trigger_motor_pid.out);
        if(shoot_control.shoot_mode == SHOOT_READY_FRIC)
        {
            shoot_control.given_current = 0;
        }
        //摩擦轮需要一个个斜波开启，不能同时直接开启，否则可能电机不转
        ramp_calc(&shoot_control.fric1_ramp, SHOOT_FRIC_PWM_ADD_VALUE);
        ramp_calc(&shoot_control.fric2_ramp, SHOOT_FRIC_PWM_ADD_VALUE);
    }

    shoot_control.fric_pwm1 = (uint16_t)(shoot_control.fric1_ramp.out);
    shoot_control.fric_pwm2 = (uint16_t)(shoot_control.fric2_ramp.out);
    shoot_fric1_on(shoot_control.fric_pwm1);
    shoot_fric2_on(shoot_control.fric_pwm2);

    return shoot_control.given_current;
}


/*射击数据更新*/
static void shoot_feedback_update(void)
{

    static fp32 speed_fliter_1 = 0.0f;
    static fp32 speed_fliter_2 = 0.0f;
    static fp32 speed_fliter_3 = 0.0f;

    //拨弹轮电机速度滤波一下
    static const fp32 fliter_num[3] = {1.725709860247969f, -0.75594777109163436f, 0.030237910843665373f};

    //二阶低通滤波
    speed_fliter_1 = speed_fliter_2;
    speed_fliter_2 = speed_fliter_3;
    speed_fliter_3 = speed_fliter_2 * fliter_num[0] + speed_fliter_1 * fliter_num[1] + (shoot_control.shoot_motor_measure->speed_rpm * MOTOR_RPM_TO_SPEED) * fliter_num[2];
    shoot_control.speed = speed_fliter_3;

    //电机圈数重置， 因为输出轴旋转一圈， 电机轴旋转 36圈，将电机轴数据处理成输出轴数据，用于控制输出轴角度
    if (shoot_control.shoot_motor_measure->ecd - shoot_control.shoot_motor_measure->last_ecd > HALF_ECD_RANGE)
    {
        shoot_control.ecd_count--;
    }
    else if (shoot_control.shoot_motor_measure->ecd - shoot_control.shoot_motor_measure->last_ecd < -HALF_ECD_RANGE)
    {
        shoot_control.ecd_count++;
    }

    if (shoot_control.ecd_count == FULL_COUNT)
    {
        shoot_control.ecd_count = -(FULL_COUNT - 1);
    }
    else if (shoot_control.ecd_count == -FULL_COUNT)
    {
        shoot_control.ecd_count = FULL_COUNT - 1;
    }

    //计算输出轴角度
    shoot_control.angle = (shoot_control.ecd_count * ECD_RANGE + shoot_control.shoot_motor_measure->ecd) * MOTOR_ECD_TO_ANGLE;

    //鼠标按键
    shoot_control.last_press_l = shoot_control.press_l;
    shoot_control.last_press_r = shoot_control.press_r;
    shoot_control.press_l = shoot_control.shoot_rc->mouse.press_l;
    shoot_control.press_r = shoot_control.shoot_rc->mouse.press_r;
    // //长按计时
    // if (shoot_control.press_l)
    // {
    //     if (shoot_control.press_l_time < PRESS_LONG_TIME)
    //     {
    //         shoot_control.press_l_time++;
    //     }
    // }
    // else
    // {
    //     shoot_control.press_l_time = 0;
    // }

    // if (shoot_control.press_r)
    // {
    //     if (shoot_control.press_r_time < PRESS_LONG_TIME)
    //     {
    //         shoot_control.press_r_time++;
    //     }
    // }
    // else
    // {
    //     shoot_control.press_r_time = 0;
    // }

    // //射击开关下档时间计时
    // if (shoot_control.shoot_mode != SHOOT_STOP && switch_is_down(shoot_control.shoot_rc->rc.s[SHOOT_RC_MODE_CHANNEL]))
    // {

    //     if (shoot_control.rc_s_time < RC_S_LONG_TIME)
    //     {
    //         shoot_control.rc_s_time++;
    //     }
    // }
    // else
    // {
    //     shoot_control.rc_s_time = 0;
    // }

    
}

//卡单，倒退
static void trigger_motor_turn_back(void)
{
    if( shoot_control.block_time < BLOCK_TIME)
    {
        shoot_control.speed_set = shoot_control.trigger_speed_set;
    }
    else
    {
        shoot_control.speed_set = -shoot_control.trigger_speed_set;
    }

    if(fabs(shoot_control.speed) < BLOCK_TRIGGER_SPEED && shoot_control.block_time < BLOCK_TIME)
    {
        shoot_control.block_time++;
        shoot_control.reverse_time = 0;
    }
    else if (shoot_control.block_time == BLOCK_TIME && shoot_control.reverse_time < REVERSE_TIME)
    {
        shoot_control.reverse_time++;
    }
    else
    {
        shoot_control.block_time = 0;
    }
}
